# -*- coding: utf-8 -*-
import os
import rospy
import time
import requests
import roslaunch
from std_msgs.msg import String
from ros_mqtt.robotsys import robotsys
from darknet_ros_msgs.msg import BoundingBoxes

class cleanwork():
    
    def __init__(self):
        self.vision_sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.vision_callback, queue_size=10)
        self.mqtt_pub = rospy.Publisher("MQTTReceiver", String, queue_size=20)
        self.code = 1
        self.msg = ''

    def start(self):
        # self.cleanwork_launch.start()
        if self.code:
            os.system('rostopic pub -1 /moveclean_cmd std_msgs/Int8 "1"')
        data = {"method": "cleanwork.start", "code": self.code, "msg": self.msg}
        return data
    
    def report(self):
        # self.cleanwork_launch.start()
        pos = {"x": robotsys.pose_x, "y": robotsys.pose_y}
        data = {"method": "cleanwork.report", "code": self.code, "msg": self.msg, "battery": robotsys.battery_ratio,
        "liquid": robotsys.liquid_ratio,"status": self.code, "pos": pos,"clean_done": [], "clean_failed": []}
        return data

    def stop(self):
        # self.cleanwork_launch.stop()
        os.system('rostopic pub -1 /clean_cmd std_msgs/Int8 "5"')
        data = {"method": "cleanwork.stop", "code": self.code, "msg": ""}
        return data

    def chargeback(self):
        os.system('rostopic pub -1 /charge_cmd std_msgs/Int8 "1"')
        rospy.sleep(180.)
        data = {"method": "cleanwork.return_finished"}
        return data
    
    def vision_callback(self,data):
        object_list = ["sports ball"]
        for box in data.bounding_boxes:
            if box.Class in object_list:
                self.code = 0
                self.msg = "There are foreign objects on the billiard table."
                data = {"method": "cleanwork.start", "code": code, "msg": msg}
                self.mqtt_pub.publish(str(data))
            else:
                self.code = 1
                self.msg = ""
